/*
*Copyright (c) Nikhil Jain.
*Project ICARUS, communications and 
*artificial intelligence.
*/



package javadrone.agent;

//~--- non-JDK imports --------------------------------------------------------

import simbad.sim.KinematicModel;

//~--- JDK imports ------------------------------------------------------------

import java.text.DecimalFormat;

import javax.media.j3d.Transform3D;

import javax.vecmath.Vector3d;

/**
 * This is the standard kinematic with two control parameters : translation and rotation.
 * Each agent use this kinematic by default.
 */
public class ExtendedKinematicModel extends KinematicModel {

    /** Rotational velocity  in radians per second */
    private Vector3d rotationalVelocity;

    /** Translational velocity  in meters per second. */
    private Vector3d translationalVelocity;

    ExtendedKinematicModel() {
        this.reset();
    }

    /** Resets all control parameters to their initial values. */
    @Override
    protected void reset() {
        rotationalVelocity    = new Vector3d(0, 0, 0);
        translationalVelocity = new Vector3d(0, 0, 0);
    }

    /**
     * Compute instant translation and rotation vectors .
     *
     * @param elapsedSecond time elapsed
     * @param rotation current rotation
     * @param instantTranslation to store translation
     * @param instantRotation to store rotation
     */
    @Override
    protected void update(double elapsedSecond, Transform3D rotation, Vector3d instantTranslation,
                          Vector3d instantRotation) {

        // System.out.println("IT: X: "+translationalVelocity.getX()*elapsedSecond+" Y: "+translationalVelocity.getY()*elapsedSecond+"Z: "+translationalVelocity.getZ()*elapsedSecond);
        instantTranslation.set(translationalVelocity.getX() * elapsedSecond,
                               translationalVelocity.getY() * elapsedSecond,
                               translationalVelocity.getZ() * elapsedSecond);

        // apply current rotation
        rotation.transform(instantTranslation);

        // perform rotation
        instantRotation.set(rotationalVelocity.getX() * elapsedSecond, rotationalVelocity.getY() * elapsedSecond,
                            rotationalVelocity.getZ() * elapsedSecond);
    }

    /**
     * Sets rotational velocity in radians per second.
     */
    public void setRotationalVelocity(Vector3d v) {

        // System.out.println("SET TRANS: X: "+v.getX()+" Y: "+v.getY()+"Z: "+v.getZ());
        rotationalVelocity.set(v.getX(), v.getY(), v.getZ());
    }

    /**
     * Sets translational velocity in meter per second.
     */
    public void setTranslationalVelocity(Vector3d v) {

        // System.out.println("SET ROT: X: "+v.getX()+" Y: "+v.getY()+"Z: "+v.getZ());
        translationalVelocity.set(v.getX(), v.getY(), v.getZ());
    }

    /**
     * Gets rotational velocity in radians per second
     */
    public Vector3d getRotationalVelocity() {
        return rotationalVelocity;
    }

    /**
     * Gets translational velocity in meter per second.
     */
    public Vector3d getTranslationalVelocity() {
        return translationalVelocity;
    }

    /** Resets all control parameters to their initial values. */
    @Override
    protected String toString(DecimalFormat format) {
        return "kinematic \t= ExtendedKinematic\n" + "rotVelocity   \t= " + format.format(rotationalVelocity.length())
               + " rad/s\n" + "transVelocity \t= " + format.format(translationalVelocity.length()) + " m/s\n";
    }
}


//~ Formatted by Jindent --- http://www.jindent.com
